/*
 2011 (C) Stefan Grufman
 MyPilot

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 any later version. see <http://www.gnu.org/licenses/>
 */
#include "defines.h"

#include <FastSerial.h>
#include "../mavlink/include/mavlink.h"        // Mavlink interface

// Used for scheduling
unsigned long prev_mavlink_ms = 0;
const unsigned long mavlink_interval = 200;

unsigned long prev_heartbeatled_ms = 0;
const unsigned long heartbeatled_interval = 1000;

// Heartbeat LED
int hbled_state = LOW;

void setup()
{
	// HEARTBEAT
	pinMode(LED_HEARTBEAT, OUTPUT);
	digitalWrite(LED_HEARTBEAT, HIGH);

	// PARAMETERS
	params_setup();

	// MAVLINK
	mavlinkcom_setup();

}

void loop()
{
	unsigned long currentMillis = millis();
	// Things done as fast as possible comes here...

	// SCHEDULED THINGS

	// MAVLINK
	if (currentMillis - prev_mavlink_ms > mavlink_interval)
	{
		prev_mavlink_ms = currentMillis;
		mavlinkcom_loop(currentMillis);
	}

	// HEARTBEAT LED
	if (currentMillis - prev_heartbeatled_ms > heartbeatled_interval)
	{
		prev_heartbeatled_ms = currentMillis;

		// TOGGLE LED
		if (hbled_state == LOW)
		{
			hbled_state = HIGH;
		}
		else
		{
			hbled_state = LOW;
		}
		digitalWrite(LED_HEARTBEAT, hbled_state);
	}

}
